#include "box2d_internal.hpp"

#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>

#include "body.hpp"
#include <gvl/math/ieee.hpp>

LUAF(body_add_circle)
{
	auto& world = check_world(L, 1);
	b2Body* body = world.check_body(L, 2);

	float32 offsx = float32(luaL_checknumber(L, 3));
	float32 offsy = float32(luaL_checknumber(L, 4));
	float32 radius = float32(luaL_checknumber(L, 5));

	b2CircleShape circle;

	circle.m_p = b2Vec2(offsx, offsy);
	circle.m_radius = radius;

	unsigned id = world.nextShapeId++;

	b2FixtureDef fixtureDef;

	fixtureDef.userData = (void*)id;
	fixtureDef.density = 1.0f;
	fixtureDef.restitution = 0.2f;
	fixtureDef.friction = 0.5f;
	fixtureDef.shape = &circle;
	if(body->GetType() == b2_staticBody)
		fixtureDef.density = 0.f;

	b2Fixture* fixture = body->CreateFixture(&fixtureDef);

	world.aliveShapes.insert(NumberedIndex<void*>(id, fixture));

	lua_pushinteger(L, id);
	return 1;
}

LUAF(body_pos)
{
	auto& world = check_world(L, 1);
	b2Body* body = world.check_body(L, 2);

	auto& pos = body->GetPosition();

	lua_pushnumber(L, to_float(pos.x));
	lua_pushnumber(L, to_float(pos.y));
	return 2;
}

LUAF(body_orientation)
{
	auto& world = check_world(L, 1);
	b2Body* body = world.check_body(L, 2);

	auto& pos = body->GetPosition();
	float32 angle = body->GetAngle();

	lua_pushnumber(L, to_float(pos.x));
	lua_pushnumber(L, to_float(pos.y));
	lua_pushnumber(L, to_float(angle));
	return 3;
}